using System;
using System.Collections.Generic;
using System.Drawing;
using System.Text;
using Diversity.Maths;

namespace Diversity.Robotics.Navigation
{
    public class PolarObstacleDensityMap : IAvoidObstacles
    {
        private double[] _segments;
        private int _numSegments;
        private double _segmentAngle;

        private double _truncateAt;
        private int _smoothing;
        private double _valleyThreashold;
        private int _largeValley;
        private double _mapRange;

        private List<int[]> _valleys;

       // private ProbabilityUtils ProbabilityUtils;// = new ProbabilityUtils();

        public double[] Segments
        {
            get { return _segments; }
            set { _segments = value; }
        }

        public int NumSegments
        {
            get { return _numSegments; }
            set { _numSegments = value; }
        }

        public double SegmentAngle
        {
            get { return _segmentAngle; }
            set { _segmentAngle = value; }
        }

        public double TruncateAt
        {
            get { return _truncateAt; }
            set { _truncateAt = value; }
        }

        public int Smoothing
        {
            get { return _smoothing; }
            set { _smoothing = value; }
        }

        public double ValleyThreashold
        {
            get { return _valleyThreashold; }
            set { _valleyThreashold = value; }
        }

        public int LargeValley
        {
            get { return _largeValley; }
            set { _largeValley = value; }
        }

        public List<int[]> Valleys
        {
            get { return _valleys; }
            set { _valleys = value; }
        }

        public double MapRange
        {
            get { return _mapRange; }
            set { _mapRange = value; }
        }

        public PolarObstacleDensityMap(int numSegments)
        {
            _numSegments = numSegments;
            _segmentAngle = (Math.PI*2)/numSegments;
            _segments = new double[_numSegments];
        }

        public void SetVFHParams(double truncate, int smoothing, double valleyThreashold, int largeValleySize, double mapRange)
        {
            _truncateAt = truncate;
            _smoothing = smoothing;
            _valleyThreashold = valleyThreashold;
            _largeValley = largeValleySize;
            _mapRange = mapRange;
        }

        public void PopulateFromMap(Map m, Pose2D state)
        {
            int pixelRange = (int)Math.Round(_mapRange / m.Scale);
            int scaledStateX = (int) Math.Round(state.X/m.Scale);
            int scaledStateY = (int) Math.Round(state.Y/m.Scale);
            int minX = Math.Max(0, scaledStateX-pixelRange);
            int maxX = Math.Min(m.Width, scaledStateX + pixelRange);
            int minY = Math.Max(0, scaledStateY - pixelRange);
            int maxY = Math.Min(m.Height, scaledStateY + pixelRange);
            _segments = new double[_numSegments];

            for (int x=minX; x<maxX; x++)
            {
                for (int y = minY; y < maxY; y++)
                {
                    double occ = ProbabilityUtils.LogOddsToProbability(m.Occupancy[x, y]);
                   // Console.Out.WriteLine("occ: " + occ + " m.Occupancy[x, y] " + m.Occupancy[x, y] + " ,x: " + x + " , y: " + y) ;
                    if (occ > 0.6)
                    {
                        double angle = Math.Atan2(y - scaledStateY, x - scaledStateX);
                        int segmentIndex = GetSegmentIndex(angle);
                        _segments[segmentIndex] += occ;
                    }
                }
            }
        }

        public void PrepareValleys()
        {
            Truncate();
            Smooth();
            Normalize();
            FindValleys();
        }

        private int GetSegmentIndex(double angle)
        {
            if (angle < 0)
            {
                angle += Math.PI*2;
            }
            if (angle > Math.PI * 2 - (_segmentAngle / 2))
            {
                return _numSegments - 1;
            }
            else
            {
                return (int) Math.Floor((angle + (_segmentAngle/2))/_segmentAngle);
            }
        }

        private double GetAngle(int segmentIndex)
        {
            double angle = segmentIndex*_segmentAngle - (_segmentAngle/2);
            if (angle > Math.PI)
            {
                angle = angle - Math.PI*2;
            }
            return angle;
        }

        private void Truncate()
        {
            for (int i = 0; i < _segments.Length; i++)
            {
                if (_segments[i] > _truncateAt)
                {
                    _segments[i] = _truncateAt;
                }
            }
        }


        private void Normalize()
        {
            _segments = ProbabilityUtils.Normalize(_segments,Normalization.MaxToOne);
        }

        private void Smooth()
        {
            double[] smoothedM = new double[_segments.Length];
            
            //   1h-4  2h-3  3h-2  4h-1  5h  4h+1 3h+2 2h+3 1h+4
            for (int i = 0; i < _segments.Length; i++)
            {
                double workingTotal =0 ;
                int factor;
                int index;
                int numReadings = 0;
                for (int j = -_smoothing + 1; j < _smoothing; j++)
                {
                    index = FixSegmentIndex(i - j);
                    factor = _smoothing - Math.Abs(j);
                    numReadings += factor;
                    workingTotal += _segments[index] * factor;
                }
                smoothedM[i] = workingTotal / numReadings;
            }
            _segments = smoothedM;
        }

        private int FixSegmentIndex(int suspectSegmentIndex)
        {
            while (suspectSegmentIndex >= _numSegments)
            {
                suspectSegmentIndex = suspectSegmentIndex - _numSegments;
            }
            while (suspectSegmentIndex < 0)
            {
                suspectSegmentIndex = suspectSegmentIndex + _numSegments;
            }
            return suspectSegmentIndex;
        }

        private void FindValleys()
        {
            List<int> emptySegments = new List<int>();
            for (int i =0; i<_segments.Length; i++)
            {
                if (_segments[i] < _valleyThreashold)
                {
                    emptySegments.Add(i);
                }
            }
           // Console.Out.WriteLine("Found " + emptySegments.Count + " empty segments");
          //  foreach (int i in emptySegments)
          //  {
           //     Console.Out.WriteLine(i.ToString());
          //  }
            List<int[]> valleys = new List<int[]>();
            
            // merge contiguous
            int index = 0; 
            while (index<emptySegments.Count)
            {
                int valleyStart = emptySegments[index];
                int valleyEnd = valleyStart;
                int testIndex = index + 1;
                while(testIndex < emptySegments.Count)
                {
                    if (emptySegments[testIndex++]-1 == valleyEnd)
                    {
                        valleyEnd++;
                    } else
                    {
                        break;
                    }
                }
                valleys.Add(new int[] {valleyStart, valleyEnd});
                index=testIndex-1;
                if(index == emptySegments.Count-1)
                {
                    break;
                }
            }

            // now try to merge the first and last valleys
            if (valleys.Count > 1)
            {
                if (valleys[0][0] == 0 && valleys[valleys.Count-1][1] == _numSegments-1)
                {
                    // merge them by deleting the first one and changing the end of the last
                    valleys[valleys.Count - 1][1] = valleys[0][1];
                    valleys.RemoveAt(0);
                }
            }

            _valleys = valleys;
        }

        public Velocity FindSafeVelocity(Velocity vel)
        {
            double desiredAngle = vel.Rotation; // probably in the +/- Math.PI range
            int segmentIndex = GetSegmentIndex(desiredAngle);
          //  Console.Out.WriteLine("Search Index is " + segmentIndex);
            // find the valleys
           
            if (_valleys.Count > 0)
            {
                if (_valleys.Count == 1 && _valleys[0][0] == 0 && _valleys[0][1]==_numSegments-1)
                {
                    // just one massive valley
                    return vel;
                }
                int closestDistance = _numSegments + 1; // something big
                int closestValley =0;
                int closestValleyEdge =0;
                for(int v=0; v<_valleys.Count; v++)
                {
                    int[] ints = _valleys[v];
                    for(int i=0; i<ints.Length; i++)
                    {
                        int distance = GetCircularDistance(segmentIndex ,ints[i]);
                        if (distance < closestDistance)
                        {
                            closestDistance = distance;
                            closestValley = v;
                            closestValleyEdge = i;
                        }
                        if (distance == 0)
                        {
                            break;
                        }
                    }

                    if (segmentIndex >= ints[0] && segmentIndex <= ints[1])
                    {
                        // within valley, so break
                        break;
                    }
                }
                
              //  Console.Out.WriteLine("Closest valley is " + closestValley + " closest edge is " + closestValleyEdge + " distance is " + closestDistance + " segment: "  + _valleys[closestValley][closestValleyEdge]);

                if (GetValleySize(_valleys[closestValley]) <= _largeValley)
                {
                    // small valley, so find middle
                   // Console.Out.WriteLine("Small valley " + GetValleySize(_valleys[closestValley]));
                    double result = GetMedianAngle(_valleys[closestValley]);
                    return new Velocity(vel.Translation,result);
                } else
                {
                    // big valley so find edge find angle of 3 from edge
                  //  int desiredIndex;
                    double result;
                    if (closestValleyEdge == 0)
                    {
                        // to the right
                        result =
                            GetMedianAngle(
                                new int[]
                                    {
                                        _valleys[closestValley][closestValleyEdge],
                                        _valleys[closestValley][closestValleyEdge] + _largeValley
                                    });
                       
                    } else
                    {
                        // to the left
                        result =
                            GetMedianAngle(
                                new int[]
                                    {
                                        _valleys[closestValley][closestValleyEdge] - LargeValley,
                                        _valleys[closestValley][closestValleyEdge]
                                    });
                        
                    }
                  //  Console.Out.WriteLine("Using segment: " + desiredIndex);
                   // double result = GetAngle(desiredIndex);
                    return new Velocity(vel.Translation, result);

                }
                

            } else
            {
               // Console.Out.WriteLine("No valleys");
                return new Velocity(0,0);
            }
        }

        private double GetMedianAngle(int[] valley)
        {
            double startAngle = GetAngle(valley[0]);
            double endAngle = GetAngle(valley[1]);

            double absDif = Math.Abs(startAngle - endAngle);
            double av = (startAngle + endAngle)/2;
            if (absDif > Math.PI)
            {
                av = av + Math.PI;
               // Console.Out.WriteLine("Corrected " + ((startAngle + endAngle)/2) + "to " + av);
            }
            if (av < -Math.PI)
            {
                av = Math.PI *2 + av;
              //  Console.Out.WriteLine("Inverted angle");
            } else if ( av > Math.PI)
            {
                av = av - Math.PI *2;
            }
            //Console.Out.WriteLine("Returning median angle between " + startAngle + " and " + endAngle + " absdif " + absDif + " av " + av);
            if (av > Math.PI * 2)
            {
                Console.Out.WriteLine("GetMedian Angle returning excesive angle" );
            }
            return av;
        }

        private int GetValleySize(int[] valley)
        {
            int dist = valley[1] - valley[0];
            if (dist < 0)
            {
                dist += _numSegments;
            }
            return dist;
        }

        private int GetCircularDistance(int index1, int index2)
        {
            int dist = Math.Abs(index1 - index2);
            if (dist > _numSegments/2)
            {
                dist = _numSegments - dist;
            }
            return dist;
        }

        public override string ToString()
        {
            string s = "PolarObstacleDensityMap : ";
            foreach (double d in _segments)
            {
                s += d.ToString("0.000") + ", ";    
            }
            if (_valleys != null)
                foreach (int[] ints in _valleys)
                {
                    s += "valley: " + ints[0] + " to " + ints[1] + ", ";
                }
            return s;
        }
	
        public Image DrawToImage(Image img)
        {
            double max = ProbabilityUtils.GetMax(_segments);
            if (max > 0)
            {
                double multiplier = (img.Width/2)/max;
                int ox = img.Width/2;
                int oy = img.Height/2;

                Graphics g = Graphics.FromImage(img);
                for (int i = 0; i < _segments.Length; i++)
                {
                    int endX = ox + (int)Math.Round(multiplier * Math.Cos(i * _segmentAngle ) * _segments[i]);
                    int endY = oy + (int)Math.Round(multiplier * Math.Sin(i * _segmentAngle ) * _segments[i]);
                    g.DrawLine(Pens.Blue, ox, oy, endX, endY);
                }
                g.Save();
                g.Dispose();
            }

            return img;
        }

        public Image DrawValleysAndDirections(Image img, Velocity desiredVelocity)
        {
            if (_valleys.Count > 0)
            {
                int endX;
                int endY;
                int ox = img.Width/2;
                int oy = img.Height/2;
                int lineLength = ox - 10;
                Graphics g = Graphics.FromImage(img);
                foreach (int[] ints in _valleys)
                {
                    endX = ox + (int) Math.Round(lineLength*Math.Cos(ints[0]*_segmentAngle - _segmentAngle/2));
                    endY = oy + (int) Math.Round(lineLength*Math.Sin(ints[0]*_segmentAngle - _segmentAngle/2));
                    g.DrawLine(Pens.Green, ox, oy, endX, endY);
                    endX = ox + (int) Math.Round(lineLength*Math.Cos(ints[1]*_segmentAngle + _segmentAngle/2));
                    endY = oy + (int) Math.Round(lineLength*Math.Sin(ints[1]*_segmentAngle + _segmentAngle/2));
                    g.DrawLine(Pens.Blue, ox, oy, endX, endY);
                }
                endX = ox + (int) Math.Round(lineLength*Math.Cos(desiredVelocity.Rotation));
                endY = oy + (int) Math.Round(lineLength*Math.Sin(desiredVelocity.Rotation));
                g.DrawLine(Pens.Black, ox, oy, endX, endY);

                Velocity vel =
                    FindSafeVelocity(
                        new Velocity(desiredVelocity.Translation, desiredVelocity.Rotation));

                endX = ox + (int) Math.Round((lineLength/2) *Math.Cos(vel.Rotation));
                endY = oy + (int) Math.Round((lineLength/2) *Math.Sin(vel.Rotation));

                g.DrawLine(Pens.Red, ox, oy, endX, endY);
                g.Save();
                g.Dispose();
            }
            return img;
        }

        public Velocity GetVelocity(Map map, Pose2D state,
                                                      Velocity desiredVelocity)
        {
            //Console.Out.WriteLine("********** GET VELOCITY *****************");
            PopulateFromMap(map,state);
            PrepareValleys();
          //  foreach (int[] ints in Valleys)
           // {
           //     Console.Out.WriteLine("Valley: " + ints[0] + " to " + ints[1]);
           // }

          //  Console.Out.WriteLine("Robot is : " + state.ToString() + " desired vel: " + desiredVelocity.ToString());
      
            // here we need to cheat and return the velocity that would turn the robot to 
            // the desired rotation
            Velocity vel =
                FindSafeVelocity(new Velocity(desiredVelocity.Translation, state.Theta));
            double rotation = vel.Rotation - state.Theta;
            while (rotation >= Math.PI)
            {
                rotation = rotation - Math.PI*2;
            }
            while (rotation <= -Math.PI)
            {
                rotation = rotation + Math.PI * 2;
            }
             //   Console.Out.WriteLine("Robot is : " + state.ToString() + " vel: " + vel.ToString() + " rot: " + rotation);

            return  new Velocity(desiredVelocity.Translation, rotation);
        }
    }
}
